// g2o - General Graph Optimization
// Copyright (C) 2012 R. Kümmerle
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <unordered_set>
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/solvers/structure_only/structure_only_solver.h"
#include "g2o/stuff/sampler.h"
#include "g2o/types/sba/types_six_dof_expmap.h" // IWYU pragma: keep
#include "g2o/types/sba/edge_project_p2mc.h"
#include "g2o/stuff/command_args.h"
#include "types.hpp"
#if defined G2O_HAVE_CHOLMOD
G2O_USE_OPTIMIZATION_LIBRARY(cholmod);
#else
G2O_USE_OPTIMIZATION_LIBRARY(eigen);
#endif

G2O_USE_OPTIMIZATION_LIBRARY(dense);

using std::cout;

using PointVector2D =
    std::vector<g2o::Vector2, Eigen::aligned_allocator<g2o::Vector2>>;
using PointVector3D =
    std::vector<g2o::Vector3, Eigen::aligned_allocator<g2o::Vector3>>;

double errorOfSolution(const PointVector2D &points,
                       const Eigen::Vector3d &circle)
{
  const Eigen::Vector2d center = circle.head<2>();
  const double radius = circle(2);
  double error = 0.;
  for (const auto &point : points)
  {
    const double distance = (point - center).norm() - radius;
    error += distance * distance;
  }
  return error;
}

int last_main(int argc, char **argv)
{
  int numPoints, resolution = 2048;
  int maxIterations, numExperiment;
  bool verbose;
  double focal_length, pixel_size;
  double PIXEL_NOISE, OUTLIER_RATIO;
  bool ROBUST_KERNEL, STRUCTURE_ONLY, debug;
  g2o::CommandArgs arg;
  arg.param("numPoints", numPoints, 100,
            "number of 3d true points");
  arg.param("maxIterations", maxIterations, 100, "perform n BA iterations");
  arg.param("v", verbose, false, "verbose output of the optimization process");
  arg.param("focal_length", focal_length, 25., "focal length of the camera in minimeters");
  arg.param("pixel_size", pixel_size, 5.5e-6, "size of a pixel in meters");
  arg.param("PIXEL_NOISE", PIXEL_NOISE, 0, "noise in image space");
  arg.param("OUTLIER_RATIO", OUTLIER_RATIO, 0.1, "ratio of outliers");
  arg.param("ROBUST_KERNEL", ROBUST_KERNEL, true, "use robust kernel");
  arg.param("STRUCTURE_ONLY", STRUCTURE_ONLY, false, "use structure-only solver");
  arg.param("numExperiment", numExperiment, 500, "number of experiments to run");
  arg.param("debug", debug, false, "print debug information");
  arg.parseArgs(argc, argv);
  cout << "PIXEL_NOISE: " << PIXEL_NOISE << '\n';
  cout << "OUTLIER_RATIO: " << OUTLIER_RATIO << '\n';
  cout << "ROBUST_KERNEL: " << ROBUST_KERNEL << '\n';
  cout << "STRUCTURE_ONLY: " << STRUCTURE_ONLY << '\n';
  // generate random data
  g2o::Sampler::seedRand();
  std::vector<g2o::Vector3> points_3d;

  for (int i = 0; i < numPoints; ++i)
  {
    points_3d.emplace_back(g2o::Sampler::uniformRand(-80e3, 80e3),
                           g2o::Sampler::uniformRand(-80e3, 80e3),
                           g2o::Sampler::uniformRand(-2e3, 4e3));
  }
  // 设置虚拟相机用于生成准确的2D点
  std::vector<g2o::SE3Quat> true_poses;
  std::vector<g2o::SE3Quat> obse_poses;
  Eigen::Vector2d principal_point(resolution / 2., resolution / 2.);
  auto cam_params = std::make_shared<MyCameraParameters>(
      focal_length / 1e3 / pixel_size, principal_point, 0.);
  cam_params->setId(0);

  // setup the solver
  g2o::SparseOptimizer optimizer;
  // allocate the solver
  g2o::OptimizationAlgorithmProperty solverProperty;
  optimizer.setAlgorithm(
      g2o::OptimizationAlgorithmFactory::instance()->construct("lm_fix6_3",
                                                               solverProperty));
  if (!optimizer.addParameter(cam_params))
  {
    assert(false);
  }
  cam_params = std::make_shared<MyCameraParameters>(
      focal_length / 1e3 / pixel_size, principal_point, 0.);
  cam_params->setId(0);
  int vertex_id = 0;
  g2o::Vector3 sum_trans_diff(0, 0, 0);
  g2o::Vector3 true_trans(0, 0, 300e3);
  for (int i = 0; i < numExperiment; ++i)
  {
    g2o::Quaternion q(1, g2o::Sampler::uniformRand(-1, 1) * 1e-4, g2o::Sampler::uniformRand(-1, 1) * 1e-4, g2o::Sampler::uniformRand(-1, 1) * 1e-4);
    q = q.normalized();
    g2o::Vector3 trans = true_trans + g2o::Vector3(g2o::Sampler::gaussRand(0., 1),
                                                   g2o::Sampler::gaussRand(0., 1),
                                                   g2o::Sampler::gaussRand(0., 1)) *
                                          1e3;
    g2o::Vector3 trans_diff = trans - true_trans;
    sum_trans_diff += trans_diff.cwiseProduct(trans_diff);
    trans = -q.toRotationMatrix() * trans;
    g2o::SE3Quat true_pose(q, true_trans);
    g2o::SE3Quat obse_pose(q, trans);
    // 用普通的位姿相机顶点即可，不需要自定义相机顶点
    auto v_se3 = std::make_shared<VertexSE3ExpmapOnlyTrans>();
    v_se3->setId(vertex_id);
    // 这一步非常关键，是为了让节点参与优化（调整）因此必须设置为false，如果要让这个节点（相机位姿）固定，就设置为true
    v_se3->setFixed(false);
    // 每增加一个节点，需要将节点id加1，全局共用同一个id顺序
    v_se3->setEstimate(obse_pose);
    /*将待优化的相机加入至优化图中*/
    optimizer.addVertex(v_se3);
    true_poses.push_back(true_pose);
    obse_poses.push_back(obse_pose);
    vertex_id++;
  }

  /*全部三维点作结点的序号*/
  int point_id = vertex_id;
  std::unordered_map<int, int> pointid_2_trueid;
  std::unordered_set<int> inliers;
  for (int i = 0; i < numPoints; ++i)
  {
    // 设置带有噪声的3D点
    auto v_p = std::make_shared<g2o::VertexPointXYZ>();
    v_p->setId(point_id);
    v_p->setMarginalized(true);
    v_p->setEstimate(points_3d.at(i));
    // TODO 三维点应当是准确的，不应该加入噪声
    int num_obs = 0;
    for (auto &obse_pose : obse_poses)
    {
      // 本步的解释：先将3D点投影到相机坐标系下，再投影到图像平面上
      g2o::Vector3 cam_points_3d = obse_pose.map(points_3d.at(i));
      g2o::Vector2 z = cam_params->cam_map(cam_points_3d);
      if (debug)
      {
        // 查看相机内参
        cout << "Camera parameters:\n";
        double f = cam_params->param().focal_length;
        double pp_x = cam_params->param().principle_point[0];
        double pp_y = cam_params->param().principle_point[1];
        double x = f * cam_points_3d[0] / cam_points_3d[2] + pp_x;
        double y = f * cam_points_3d[1] / cam_points_3d[2] + pp_y;
        cout << f << ' ' << pp_x << " " << pp_y << ' ' << cam_params->param().baseline << '\n';
        cout << "True 3D point:\n";
        cout << points_3d.at(i).transpose() << '\n';
        cout << "Camera 3D point:\n";
        cout << cam_points_3d.transpose() << '\n';
        cout << "True 2D point:\n";
        cout << z.transpose() << '\n';
        cout << "Observe 2D point:\n";
        cout << x << " " << y << '\n';
        cout << "Observe translation:\n";
        cout << obse_pose.translation().transpose() << '\n';
        cout << "Observe rotation:\n";
        cout << obse_pose.rotation().toRotationMatrix() << '\n';
      }
      if (z[0] >= 0 && z[1] >= 0 && z[0] < resolution && z[1] < resolution)
      {
        ++num_obs;
      }
    }
    if (num_obs >= 2)
    {
      optimizer.addVertex(v_p);
      for (size_t j = 0; j < obse_poses.size(); ++j)
      {
        g2o::Vector3 cam_points_3d = obse_poses.at(j).map(points_3d.at(i));
        /*在图像平面像元范围内*/
        g2o::Vector2 z = cam_params->cam_map(cam_points_3d);
        if (z[0] >= 0 && z[1] >= 0 && z[0] < resolution && z[1] < resolution)
        {
          double sam = g2o::Sampler::uniformRand(0., 1.);
          if (sam < OUTLIER_RATIO)
          {
            z = g2o::Vector2(g2o::Sampler::uniformRand(0, resolution), g2o::Sampler::uniformRand(0, resolution));
          }
          z += g2o::Vector2(g2o::Sampler::gaussRand(0., PIXEL_NOISE),
                            g2o::Sampler::gaussRand(0., PIXEL_NOISE));
          auto e = std::make_shared<EdgeSE3ProjectXYZOnlyTrans>();
          e->setVertex(0, v_p);
          e->setVertex(1, optimizer.vertices().find(j)->second);
          e->setMeasurement(z);
          e->information() = g2o::Matrix2::Identity();
          if (ROBUST_KERNEL)
          {
            e->setRobustKernel(std::make_shared<g2o::RobustKernelHuber>());
          }
          e->setParameterId(0, 0);
          optimizer.addEdge(e);
        }
      }

      pointid_2_trueid.insert(std::make_pair(point_id, i));
      ++point_id;
    }
  }
  cout << '\n';
  optimizer.initializeOptimization();
  optimizer.setVerbose(verbose);
  if (STRUCTURE_ONLY)
  {
    g2o::StructureOnlySolver<3> structure_only_ba;
    cout << "Performing structure-only BA:\n";
    g2o::OptimizableGraph::VertexContainer points;
    for (const auto &it : optimizer.vertices())
    {
      auto v =
          std::static_pointer_cast<g2o::OptimizableGraph::Vertex>(it.second);
      if (v->dimension() == 3)
        points.push_back(v);
    }
    structure_only_ba.calc(points, maxIterations);
  }
  // optimizer.save("test.g2o");
  cout << '\n';
  cout << "Performing full BA:\n";
  optimizer.optimize(maxIterations);
  cout << "Translation error before optimisation:\n";
  cout << (sum_trans_diff / numExperiment).cwiseSqrt().transpose() << '\n';
  // 清零
  sum_trans_diff.setZero();
  for (int i = 0; i < numExperiment; i++)
  {
    auto v_it = optimizer.vertices().find(i);
    auto *v_p = dynamic_cast<VertexSE3ExpmapOnlyTrans *>(v_it->second.get());
    /*估计的位置，这里是世界坐标系下的位置，所以！！！切记！！！不要漏乘旋转矩阵*/
    g2o::Vector3 esti_pose = -v_p->estimate().rotation().toRotationMatrix() * v_p->estimate().translation();
    g2o::Vector3 err_diff = true_poses[i].translation() - esti_pose;
    sum_trans_diff += err_diff.cwiseProduct(err_diff);
  }
  cout << "Translation error after optimisation:\n";
  cout << (sum_trans_diff / numExperiment).cwiseSqrt().transpose() << '\n';
  return 0;
}
int main(int argc, char **argv)
{
  return last_main(argc, argv);
}
